//=============================================================================
//     _   _                               ____                _  _          
//    | | | |  __ _  _ __   _ __   _   _  / ___|  _ __   _ __ (_)| |_   ___ 
//    | |_| | / _` || '_ \ | '_ \ | | | | \___ \ | '_ \ | '__|| || __| / _ \
//    |  _  || (_| || |_) || |_) || |_| |  ___) || |_) || |   | || |_ |  __/
//    |_| |_| \__,_|| .__/ | .__/  \__, | |____/ | .__/ |_|   |_| \__| \___|
//                  |_|    |_|     |___/         |_|                         
//
//                     HappySprite - We make sprites happy
//
// Copyright (c) 2007 by Tank Monkey Games
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//=============================================================================

using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace HappySprite
{
    public static class QuaternionExtensions
    {
        const float ZeroTolerance = 1e-06f;

        public static void ToAxisAngle(ref Quaternion q, out Vector3 axis, out float angle)
        {
            // The quaternion representing the rotation is
            //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

            float sqrLength = q.X * q.X + q.Y * q.Y + q.Z * q.Z;

            if (sqrLength > ZeroTolerance)
            {
                angle = 2f * Trigonometry.Acos(q.W);
                float invLength = (float)(1.0 / Math.Sqrt(sqrLength));

                axis = new Vector3(
                    q.X * invLength,
                    q.Y * invLength,
                    q.Z * invLength);
            }
            else
            {
                // angle is 0 (mod 2*pi), so any axis will do
                angle = 0f;
                axis = Vector3.UnitZ;
            }
        }

        public static float ToPitch(ref Quaternion q)
        {
            float yz = q.Y * q.Z;
            float xw = q.X * q.W;

            return Trigonometry.Asin(-2f * (yz - xw));
        }

        public static float ToRoll(ref Quaternion q)
        {
            float xx = q.X * q.X;
            float yy = q.Y * q.Y;
            float ww = q.W * q.W;
            float zz = q.Z * q.Z;
            float zw = q.Z * q.W;
            float xy = q.X * q.Y;

            return Trigonometry.Atan2(2f * (zw + xy), ww - zz - xx + yy);
        }

        public static float ToYaw(ref Quaternion q)
        {
            float xx = q.X * q.X;
            float yy = q.Y * q.Y;
            float ww = q.W * q.W;
            float zz = q.Z * q.Z;
            float xz = q.X * q.Z;
            float yw = q.Y * q.W;

            return Trigonometry.Atan2(2f * (xz + yw), ww + zz - xx - yy);
        }

        public static void ToYawPitchRoll(ref Quaternion q, out float yaw, out float pitch, out float roll)
        {
            float xx = q.X * q.X;
            float yy = q.Y * q.Y;
            float ww = q.W * q.W;
            float zz = q.Z * q.Z;
            float xy = q.X * q.Y;
            float xz = q.X * q.Z;
            float xw = q.X * q.W;
            float yz = q.Y * q.Z;
            float yw = q.Y * q.W;
            float zw = q.Z * q.W;

            pitch = Trigonometry.Asin(-2f * (yz - xw));
            roll = Trigonometry.Atan2(2f * (zw + xy), ww - zz - xx + yy);
            yaw = Trigonometry.Atan2(2f * (xz + yw), ww + zz - xx - yy);
        }
    }
}
